#include "stdafx.h"
#include "VectorMath.h"
#include "Vectors.h"
#include <mmintrin.h>

_declspec(align(16)) float outval[4];

_declspec(align(16)) float vector_a[4];
_declspec(align(16)) float vector_b[4];

_declspec(align(16)) float vector_zero[4]
{
	0.f, 0.f, 0.f, 0.f
};

void ClearVectors(void)
{
	memset(vector_a, 0, 16);
	memset(vector_b, 0, 16);
}

#pragma region Vector Work
void AddVectors(void)
{
	__m128 v1 = _mm_load_ps(vector_a);
	__m128 v2 = _mm_load_ps(vector_b);
	v1 = _mm_add_ps(v1, v2);
	_mm_store_ps(outval, v1);
}

void SubtractVectors(void)
{
	__m128 v1 = _mm_load_ps(vector_a);
	__m128 v2 = _mm_load_ps(vector_b);
	v1 = _mm_sub_ps(v1, v2);
	_mm_store_ps(outval, v1);
}

// multiply vector_a by 'f'
void MultVectorAndFloat(float f)
{
	// load 'f' parameter into xm register
	__m128 F = _mm_set1_ps(f);
	__m128 v = _mm_load_ps(vector_a);
	v = _mm_mul_ps(v, F);
	_mm_store_ps(outval, v);
}

void DotVectors(void)
{
	// load a and b in to  memory
	__m128 aa = _mm_load_ps(vector_a);
	__m128 bb = _mm_load_ps(vector_b);

	// multiply a and b
	__m128 v = _mm_mul_ps(aa, bb);
	__m128 zero = _mm_setzero_ps();
	// horazontal add 1
	__m128 result = _mm_hadd_ps(v, zero);
	// horazontal add 2
	result = _mm_hadd_ps(result, zero);
	
	// copy result into outval
	_mm_store_ps(outval, result);
}

void CrossVectors(void)
{
	__m128 v1 = _mm_load_ps(vector_a);
	__m128 v2 = _mm_load_ps(vector_b);

	// storage array
	_declspec(align(16)) float e[4];
#define CLEAR_MEM memset(e, 0, 16)
	// zero vector
	__m128 zero = _mm_load_ps(Vector4(0.f));

	// compute 'i' colomn
	CLEAR_MEM;
	e[0] = v1.m128_f32[1];
	e[1] = v2.m128_f32[1];
	__m128 i1 = _mm_load_ps(e);
	CLEAR_MEM;
	e[0] = v2.m128_f32[2];
	e[1] = v1.m128_f32[2];
	__m128 i2 = _mm_load_ps(e);

	__m128 i = _mm_mul_ps(i1, i2);
	i = _mm_hsub_ps(i, zero);

	// compute 'j' colomn
	CLEAR_MEM;
	e[0] = v1.m128_f32[0];
	e[1] = v2.m128_f32[0];
	__m128 j1 = _mm_load_ps(e);
	CLEAR_MEM;
	e[0] = v2.m128_f32[2];
	e[1] = v1.m128_f32[2];
	__m128 j2 = _mm_load_ps(e);

	__m128 j = _mm_mul_ps(j1, j2);
	j = _mm_hsub_ps(j, zero);

	// compute 'k' colomn
	CLEAR_MEM;
	e[0] = v1.m128_f32[0];
	e[1] = v2.m128_f32[0];
	__m128 k1 = _mm_load_ps(e);
	CLEAR_MEM;
	e[0] = v2.m128_f32[1];
	e[1] = v1.m128_f32[1];
	__m128 k2 = _mm_load_ps(e);
	
	__m128 k = _mm_mul_ps(k1, k2);
	k = _mm_hsub_ps(k, zero);

	// compute i - j + k
	outval[0] = i.m128_f32[0];
	outval[1] = -j.m128_f32[0];
	outval[2] = k.m128_f32[0];
	outval[3] = 0.f;
#undef CLEAR_MEM
}

bool CompareVectors(void)
{
	__m128 v1 = _mm_load_ps(vector_a);
	__m128 v2 = _mm_load_ps(vector_b);
	v1 = _mm_cmpeq_ps(v1, v2);

	return ((int)v1.m128_f32[0] & (int)v1.m128_f32[1] & (int)v1.m128_f32[2] & (int)v1.m128_f32[3]) != 0;
}

bool CompareMatricies(Matrix* a, Matrix* b)
{
	for (int i = 0; i < 4; i++)
	{
		memcpy(vector_a, a->e + i * 4, 16);
		memcpy(vector_b, a->e + i * 4, 16);
		if (!CompareVectors())
			return false;
	}

	return true;
}

const float negative_v[4] = { -1.f, -1.f, -1.f, -1.f };

void NegateVector(void)
{
	__m128 neg1 = _mm_load1_ps(negative_v);
	__m128 v1 = _mm_load_ps(vector_a);
	v1 = _mm_mul_ps(v1, neg1);
	_mm_store_ps(vector_a, v1);
}

void NormalizeQuaternoin(void)
{
	__m128 square = _mm_mul_ps(square, square), q = _mm_load_ps(vector_a);
	__m128 zero = _mm_load_ps(vector_zero);
	square = _mm_hadd_ps(square, zero);
	square = _mm_hadd_ps(square, zero);

	__m128 vectorSquare = _mm_load_ps1(&square.m128_f32[0]);
	__m128 result = _mm_add_ps(q, vectorSquare);
	memcpy(outval, result.m128_f32, 16);
}

#pragma endregion

#pragma region Matrix Work

void AddMatricies(Matrix* a, Matrix* b, Matrix* outval)
{
	__m128 v1 = _mm_load_ps(a->m1);
	__m128 v2 = _mm_load_ps(b->m1);

	__m128 e = _mm_add_ps(v1, v2);
	_mm_store_ps(outval->m1, e);

	v1 = _mm_load_ps(a->m2);
	v2 = _mm_load_ps(b->m2);

	e = _mm_add_ps(v1, v2);
	_mm_store_ps(outval->m2, e);

	v1 = _mm_load_ps(a->m3);
	v2 = _mm_load_ps(b->m3);

	e = _mm_add_ps(v1, v2);
	_mm_store_ps(outval->m3, e);

	v1 = _mm_load_ps(a->m4);
	v2 = _mm_load_ps(b->m4);

	e = _mm_add_ps(v1, v2);
	_mm_store_ps(outval->m4, e);
}

void subtractMatricies(Matrix* a, Matrix* b, Matrix* out)
{
	__m128 v1 = _mm_load_ps(a->m1);
	__m128 v2 = _mm_load_ps(b->m1);

	__m128 e = _mm_sub_ps(v1, v2);
	_mm_store_ps(out->m1, e);

	v1 = _mm_load_ps(a->m2);
	v2 = _mm_load_ps(b->m2);

	e = _mm_sub_ps(v1, v2);
	_mm_store_ps(out->m2, e);

	v1 = _mm_load_ps(a->m3);
	v2 = _mm_load_ps(b->m3);

	e = _mm_sub_ps(v1, v2);
	_mm_store_ps(out->m3, e);

	v1 = _mm_load_ps(a->m4);
	v2 = _mm_load_ps(b->m4);

	e = _mm_sub_ps(v1, v2);
	_mm_store_ps(out->m4, e);
}

void GetMatrixColomn(Matrix* matrix, float* out, int colomn_index)
{
	out[0] = matrix->e[colomn_index + 0 * 4];
	out[1] = matrix->e[colomn_index + 1 * 4];
	out[2] = matrix->e[colomn_index + 2 * 4];
	out[3] = matrix->e[colomn_index + 3 * 4];
}
void MultMatricies(Matrix* a, Matrix* b, Matrix* out)
{
	memset(vector_a, 0, 16);
	__m128 row, colomn, _m, zero = _mm_setzero_ps();

	// loop over every row and colomn
	for (int i = 0; i < 4; i++)
	{
		row = _mm_load_ps(a->e + i * 4);
		for (int ii = 0; ii < 4; ii++)
		{
			GetMatrixColomn(b, vector_a, ii);
			colomn = _mm_load_ps(vector_a);
			_m = _mm_mul_ps(row, colomn);
			// add twice for dot product
			_m = _mm_hadd_ps(_m, zero);
			_m = _mm_hadd_ps(_m, zero);

			out->e[ii + i * 4] = _m.m128_f32[0];
		}
	}
}

void MultMatrixAndScaler(Matrix* a, float f, Matrix* out)
{
	__m128 s = _mm_load1_ps(&f);
	__m128 v = _mm_load_ps(a->m1);
	__m128 result = _mm_mul_ps(v, s);

	_mm_store_ps(out->m1, result);

	v = _mm_load_ps(a->m2);
	result = _mm_mul_ps(v, s);
	_mm_store_ps(out->m2, result);

	v = _mm_load_ps(a->m3);
	result = _mm_mul_ps(v, s);
	_mm_store_ps(out->m3, result);

	v = _mm_load_ps(a->m4);
	result = _mm_mul_ps(v, s);
	_mm_store_ps(out->m4, result);
}

// multiply 'vector_a' with 'm'
void MultVectorAndMatrix(Matrix* m)
{
	_declspec(align(16)) float e[4];
	memset(e, 0, 16);

	__m128 vector = _mm_load_ps(vector_a), temp, zero = _mm_load_ps(Vector4(0.f));
	for (int i = 0; i < 4; i++)
	{
		temp = _mm_load_ps(m->e + i * 4);
		temp = _mm_mul_ps(vector, temp);
		// dot product
		temp = _mm_hadd_ps(temp, zero);
		temp = _mm_hadd_ps(temp, zero);
		outval[i] = temp.m128_f32[0];
		memset(e, 0, 16); // clear e
	}
}

#pragma endregion

#pragma region Quaternion Work

Quaternion operator+(Quaternion& a, Quaternion& b)
{
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	AddVectors();
	return outval;
}

Quaternion operator-(Quaternion& a)
{
	memcpy(vector_a, a, 16);
	NegateVector();
	return outval;
}

Quaternion operator-(Quaternion& a, Quaternion& b)
{
	ClearVectors();
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	SubtractVectors();
	return outval;
}

float operator*(Quaternion& a, Quaternion& b)
{
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	DotVectors();
	return outval[0];
}

Quaternion operator*(Quaternion& q, float f)
{
	memcpy(vector_a, q, 16);
	MultVectorAndFloat(f);
	return outval;
}

Quaternion operator*(float f, Quaternion& q)
{
	return q * f;
}

Quaternion operator~(Quaternion& a)
{
	a.x = -a.x;
	a.y = -a.y;
	a.z = -a.z;
	return a;
}

Quaternion Quaternion::operator +=(Quaternion& b)
{
	return *this + b;
}

Quaternion Quaternion::operator -=(Quaternion& b)
{
	return *this - b;
}

void Quaternion::Normalize(void)
{
	memcpy(vector_a, this, 16);
	NormalizeQuaternoin();
	memcpy(this, vector_a, 16);
}

#pragma endregion

#pragma region Matrix on matrix action!

Matrix operator +(Matrix& a, Matrix& b)
{
	Matrix m;
	AddMatricies(&a, &b, &m);

	return m;
}

Matrix operator -(Matrix& a, Matrix& b)
{
	Matrix mm;
	subtractMatricies(&a, &b, &mm);

	return mm;
}
Matrix operator *(Matrix& a, Matrix& b)
{
	Matrix out;
	MultMatricies(&a, &b, &out);

	return out;
}
Matrix operator *(Matrix& m, float f)
{
	Matrix out;
	MultMatrixAndScaler(&m, f, &out);

	return out;
}
Matrix operator *(float f, Matrix& m)
{
	Matrix out;
	MultMatrixAndScaler(&m, f, &out);

	return out;
}

bool operator !=(Matrix& a, Matrix& b)
{
	return !CompareMatricies(&a, &b);
}

bool operator ==(Matrix& a, Matrix& b)
{
	return CompareMatricies(&a, &b);
}
#pragma endregion

#pragma region Matrix on vector action!

Vector2 operator *(Vector2& v, Matrix& m)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 8);
	vector_a[3] = 1.f;
	MultVectorAndMatrix(&m);

	return outval;
}

Vector3 operator *(Vector3& v, Matrix& m)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 12);
	vector_a[3] = 1.f;
	MultVectorAndMatrix(&m);

	return outval;
}

Vector4 operator *(Vector4& v, Matrix& m)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 16);
	MultVectorAndMatrix(&m);

	return outval;
}

#pragma endregion

#pragma region Vector on vector action!
#pragma region Add
Vector2 operator +(Vector2& a, Vector2& b)
{
	ClearVectors();
	memcpy(vector_a, a, 8);
	memcpy(vector_b, b, 8);
	AddVectors();
	return outval;
}
Vector3 operator +(Vector3& a, Vector3& b)
{
	ClearVectors();
	memcpy(vector_a, a, 12);
	memcpy(vector_b, b, 12);
	AddVectors();
	return outval;
}
Vector4 operator +(Vector4& a, Vector4& b)
{
	ClearVectors();
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	AddVectors();
	return outval;
}


Vector2& Vector2::operator +=(Vector2& other)
{
	ClearVectors();
	memcpy(vector_a, this, 8);
	memcpy(vector_b, other, 8);
	AddVectors();
	memcpy(this, outval, 8);

	return *this;
}
Vector3& Vector3::operator +=(Vector3& other)
{
	ClearVectors();
	memcpy(vector_a, this, 12);
	memcpy(vector_b, other, 12);
	AddVectors();
	memcpy(this, outval, 12);

	return *this;
}
Vector4& Vector4::operator +=(Vector4& other)
{
	ClearVectors();
	memcpy(vector_a, this, 16);
	memcpy(vector_b, other, 16);
	AddVectors();
	memcpy(this, outval, 16);

	return *this;
}
#pragma endregion

#pragma region Subtract
Vector2 operator -(Vector2& a, Vector2& b)
{
	ClearVectors();
	memcpy(vector_a, a, 8);
	memcpy(vector_b, b, 8);
	SubtractVectors();
	return outval;
}
Vector3 operator -(Vector3& a, Vector3& b)
{
	ClearVectors();
	memcpy(vector_a, a, 12);
	memcpy(vector_b, b, 12);
	SubtractVectors();
	return outval;
}
Vector4 operator -(Vector4& a, Vector4& b)
{
	ClearVectors();
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	SubtractVectors();
	return outval;
}

Vector2& Vector2::operator -=(Vector2& other)
{
	ClearVectors();
	memcpy(vector_a, this, 8);
	memcpy(vector_b, other, 8);
	SubtractVectors();
	memcpy(this, outval, 8);

	return *this;
}
Vector3& Vector3::operator -=(Vector3& other)
{
	ClearVectors();
	memcpy(vector_a, this, 12);
	memcpy(vector_b, other, 12);
	SubtractVectors();
	memcpy(this, outval, 12);

	return *this;
}
Vector4& Vector4::operator -=(Vector4& other)
{
	ClearVectors();
	memcpy(vector_a, this, 16);
	memcpy(vector_b, other, 16);
	SubtractVectors();
	memcpy(this, outval, 16);

	return *this;
}
#pragma endregion

#pragma region Dot
float operator *(Vector2& a, Vector2& b)
{
	ClearVectors();
	memcpy(vector_a, a, 8);
	memcpy(vector_b, b, 8);
	DotVectors();
	return outval[0];
}
float operator *(Vector3& a, Vector3& b)
{
	ClearVectors();
	memcpy(vector_a, a, 12);
	memcpy(vector_b, b, 12);
	DotVectors();
	return outval[0];
}
float operator *(Vector4& a, Vector4& b)
{
	ClearVectors();
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	DotVectors();
	return outval[0];
}
#pragma endregion

#pragma region Cross

Vector3 operator ^(Vector3& a, Vector3& b)
{
	ClearVectors();
	memcpy(vector_a, a, 12);
	memcpy(vector_b, b, 12);
	CrossVectors();
	return outval;
}

#pragma endregion

#pragma region Equality

bool operator ==(Vector2& a, Vector2& b)
{
	ClearVectors();
	memcpy(vector_a, a, 8);
	memcpy(vector_b, b, 8);
	return CompareVectors();
}
bool operator ==(Vector3& a, Vector3& b)
{
	ClearVectors();
	memcpy(vector_a, a, 12);
	memcpy(vector_b, b, 12);
	return CompareVectors();
}
bool operator ==(Vector4& a, Vector4& b)
{
	ClearVectors();
	memcpy(vector_a, a, 16);
	memcpy(vector_b, b, 16);
	return CompareVectors();
}

bool operator !=(Vector2& a, Vector2& b)
{
	return !(a == b);
}
bool operator !=(Vector3& a, Vector3& b)
{
	return !(a == b);
}
bool operator !=(Vector4& a, Vector4& b)
{
	return !(a == b);
}
#pragma endregion

#pragma region Negate


Vector2 operator -(Vector2& v)
{
	memset(vector_a, 0, 16);
	memcpy(v, vector_a, 8);
	NegateVector();

	return vector_a;
}

Vector3 operator -(Vector3& v)
{
	memset(vector_a, 0, 16);
	memcpy(v, vector_a, 12);
	NegateVector();

	return vector_a;	
}

Vector4 operator -(Vector4& v)
{
	memcpy(v, vector_a, 16);
	NegateVector();

	return vector_a;
}

#pragma endregion

#pragma region Distance

float Distance(Vector2& a, Vector2& b)
{
	static __m128 zero = _mm_setzero_ps();
	memset(vector_a, 0, 16);
	memset(vector_b, 0, 16);

	memcpy(vector_a, a, 8);
	memcpy(vector_b, b, 8);

	__m128 v1 = _mm_load_ps(vector_a);
	__m128 v2 = _mm_load_ps(vector_b);
	// find the vector between the two points
	v1 = _mm_sub_ps(v1, v2);
	// compute the length of v1
	v1 = _mm_mul_ps(v1, v1);
	v1 = _mm_hadd_ps(v1, zero);
	v1 = _mm_hadd_ps(v1, zero);

	return v1.m128_f32[0];
}

float Distance(Vector3& a, Vector3& b)
{
	static __m128 zero = _mm_setzero_ps();
	memset(vector_a, 0, 16);
	memset(vector_b, 0, 16);

	memcpy(vector_a, a, 12);
	memcpy(vector_b, b, 12);

	__m128 v1 = _mm_load_ps(vector_a);
	__m128 v2 = _mm_load_ps(vector_b);
	// find the vector between the two points
	v1 = _mm_sub_ps(v1, v2);
	// compute the length of v1
	v1 = _mm_mul_ps(v1, v1);
	v1 = _mm_hadd_ps(v1, zero);
	v1 = _mm_hadd_ps(v1, zero);

	return v1.m128_f32[0];
}
#pragma endregion
#pragma endregion

#pragma region Vector on scaler action!
Vector2 operator *(Vector2& v, float f)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 8);
	MultVectorAndFloat(f);
	return Vector2(outval);
}
Vector2 operator *(float f, Vector2& v)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 8);
	MultVectorAndFloat(f);
	return Vector2(outval);
}

Vector3 operator *(Vector3& v, float f)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 12);
	MultVectorAndFloat(f);
	return outval;
}
Vector3 operator *(float f, Vector3& v)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 12);
	MultVectorAndFloat(f);
	return outval;
}

Vector4 operator *(Vector4& v, float f)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 16);
	MultVectorAndFloat(f);
	return outval;
}
Vector4 operator *(float f, Vector4& v)
{
	memset(vector_a, 0, 16);
	memcpy(vector_a, v, 16);
	MultVectorAndFloat(f);
	return outval;
}
#pragma endregion

#pragma region and normaizeing...

void Vector2::Normalize(void)
{
	__m128 zero = _mm_setzero_ps();
	memset(vector_a, 0, 16);
	memcpy(vector_a, this, 8);

	__m128 v = _mm_load_ps(vector_a);
	v = _mm_mul_ps(v, v);
	v = _mm_hadd_ps(v, zero);
	v = _mm_hadd_ps(v, zero);
	v = _mm_sqrt_ps(v);
	__m128 div = _mm_load1_ps(v.m128_f32);
	v = _mm_load_ps(vector_a);
	v = _mm_div_ps(v, div);
	_mm_store_ps(vector_a, v);
	memcpy(this, vector_a, 8);
}

void Vector3::Normalize(void)
{
	__m128 zero = _mm_setzero_ps();
	memset(vector_a, 0, 16);
	memcpy(vector_a, this, 12);

	__m128 v = _mm_load_ps(vector_a);
	v = _mm_mul_ps(v, v);
	v = _mm_hadd_ps(v, zero);
	v = _mm_hadd_ps(v, zero);
	v = _mm_sqrt_ps(v);
	__m128 div = _mm_load1_ps(v.m128_f32);
	if (v.m128_f32[0] == 0.f)
		return;
	v = _mm_load_ps(vector_a);
	v = _mm_div_ps(v, div);
	_mm_store_ps(vector_a, v);
	memcpy(this, vector_a, 12);
}

void Vector4::Normalize(void)
{
	__m128 zero = _mm_setzero_ps();
	memcpy(vector_a, this, 16);

	__m128 v = _mm_load_ps(vector_a);
	v = _mm_mul_ps(v, v);
	v = _mm_hadd_ps(v, zero);
	v = _mm_hadd_ps(v, zero);
	v = _mm_sqrt_ps(v);
	__m128 div = _mm_load1_ps(v.m128_f32);
	v = _mm_load_ps(vector_a);
	v = _mm_div_ps(v, div);
	_mm_store_ps(vector_a, v);
	memcpy(this, vector_a, 16);

}

#pragma endregion

#pragma region ... also some boolean operators ...

bool operator <(Vector2& a, Vector2& b)
{
	return a.getLength() < b.getLength();
}

bool operator <(Vector3& a, Vector3& b)
{
	return a.getLength() < b.getLength();
}

bool operator <(Vector4& a, Vector4& b)
{
	return a.getLength() < b.getLength();
}
#pragma endregion